import pyb
from pyb import LED,UART,Pin
import utime
import sensor,image,time

import json

red_led  = LED(1)
green_led  = LED(2)
blue_led  = LED(3)
ir_led  = LED(4)

ping_pong_ball_threshold   = (63, 94, 15, 56, -128, 116)
ping_pong_ball_rgb =(255, 88, 0)


#1 io口输出输入
#如果不特别说明，默认包含以下头文件
p_out = Pin('P7', Pin.OUT_PP)#设置p_out为输出引脚
p_out.high()#设置p_out引脚为高
p_out.low()#设置p_out引脚为低

p_in = Pin('P7', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻
value = p_in.value() # get value, 0 or 1#读入p_in引脚的值



led = LED(1) # 红led
led.toggle()
led.on()#亮
utime.sleep(1)
led.off()#灭
utime.sleep(1)
led.on()#亮


red_led.toggle()

tim4 = pyb.Timer(4)
tim4.init(freq=1) #1hz 1s


count = 1
def call_back_4(info):
    global count
    count+=1
    red_led.toggle()

tim4.callback(call_back_4)





#10 测距
sensor.reset()                      # Reset and initialize the sensor.
sensor.set_pixformat(sensor.RGB565) # Set pixel format to RGB565 (or GRAYSCALE)
sensor.set_framesize(sensor.QVGA)   # Set frame size to QVGA (320x240)
#sensor.skip_frames(time = 10)     # Wait for settings take effect.
clock = time.clock()                # Create a clock object to track the FPS.


#测距
#lab阈值，用于对颜色进行分类或过滤
ping_gai_ball_threshold = (23, 100, 127, 36, -6, 58)
#(23, 100, 127, 36, -6, 58)
K=5000#the value should be measured

while(True):
    clock.tick() # Track elapsed milliseconds between snapshots().
    img = sensor.snapshot() # Take a picture and return the image.

    blobs = img.find_blobs([ping_gai_ball_threshold])
    if len(blobs) == 1:
        # Draw a rect around the blob.
        b = blobs[0]
        img.draw_rectangle(b[0:4]) # rect
        img.draw_cross(b[5], b[6]) # cx, cy
        Lm = (b[2]+b[3])/2
        length = K/Lm
        print("length:{}mm".format(length))